
// 模仿编码器数据

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include <iostream>
#include <chrono>
#include <string>

using namespace std;
using namespace std::chrono_literals; // 引入std::chrono_literals


// 提示用户输入消息内容和频率
int joint1_msg;
float joint2_msg, joint3_msg;

class JointPublisher : public rclcpp::Node
{
public:
    JointPublisher()
        : Node("joint_publisher_node")
    {
        // 创建发布者
        joint1_pub_ = this->create_publisher<std_msgs::msg::Int32>("/joint1_p", 10);
        joint2_pub_ = this->create_publisher<std_msgs::msg::Float64>("/joint2_p", 10);
        joint3_pub_ = this->create_publisher<std_msgs::msg::Float64>("/joint3_p", 10);

        // // 用户输入的消息内容
        // joint1_data_ = joint1_msg;
        // joint2_data_ = joint2_msg;
        // joint3_data_ = joint3_msg;

        timer_ = this->create_wall_timer(100ms, std::bind(&JointPublisher::publish_messages, this));
    }

private:
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint1_pub_;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pub_;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pub_;
    rclcpp::TimerBase::SharedPtr timer_;

    void publish_messages()
    {
        // 创建消息
        auto joint1_msg = std_msgs::msg::Int32();
        joint1_msg.data = 40;

        auto joint2_msg = std_msgs::msg::Float64();
        joint2_msg.data = 268.9;

        auto joint3_msg = std_msgs::msg::Float64();
        joint3_msg.data = 206.1;

        // 发布消息
        joint1_pub_->publish(joint1_msg);
        joint2_pub_->publish(joint2_msg);
        joint3_pub_->publish(joint3_msg);

        // 输出日志
        RCLCPP_INFO(this->get_logger(), "Published: joint1_p=%d, joint2_p=%.2f, joint3_p=%.2f",
                    joint1_msg.data, joint2_msg.data, joint3_msg.data);
    }
};

int main(int argc, char **argv)
{
    // 初始化ROS节点
    rclcpp::init(argc, argv);
    // 创建发布者节点
    auto joint_publisher = std::make_shared<JointPublisher>();
    // 启动ROS通信循环
    rclcpp::spin(joint_publisher);
    // 清理资源
    rclcpp::shutdown();

    return 0;
}

